#include "drv_chassis.h"
#include "bsp_can.h"
#include "bsp_dbus.h"
#include "math.h"
#include "apl_pid.h"

#define realspeedk 3.75f

PID wheel[4];

void PID_chassis_init() {
	for(char i = 0; i < 4; ++i)
		PID_init_speed(&wheel[i], CAN_3508_MAXI, &k_speed_chassis);
}

short move_mode;

float vx = 0, vy = 0, vw = 0;
float motor_speed[4] = {0};
short  motor_input[4] = {0};

void Chassis_init(void) {
	PID_chassis_init();
	vx = vy = vw = 0;
	for(char i = 0; i < 4; ++i) motor_speed[i] = 0, motor_input[i] = 0;
	CAN_cmd_chassis(motor_input);
	HAL_Delay(500);
}

void Chassis_RC(void) {
	vx = rc_ctl.rc.ch[1] - 1024;
	vy = rc_ctl.rc.ch[0] - 1024;
	vw = rc_ctl.rc.ch[2] - 1024;
	vw *= -1;
	vx *= realspeedk;
	vy *= realspeedk;
	vw *= realspeedk;
}	


void Chassis_set_speed(float vx_set, float vy_set, float vw_set, float *wheel_speed) {
	vx = vx_set, vy = vy_set, vw = vw_set;
	
	wheel_speed[0] = -vx_set - vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * vw_set; 
	wheel_speed[1] = vx_set - vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * vw_set; 
	wheel_speed[2] = vx_set + vy_set + (-CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * vw_set; 
	wheel_speed[3] = -vx_set + vy_set + (-CHASSIS_WZ_SET_SCALE - 1.0f) * MOTOR_DISTANCE_TO_CENTER * vw_set;
	
	float mm = fmax(fmax(fabs(wheel_speed[0]), fabs(wheel_speed[1])), fmax(fabs(wheel_speed[2]), fabs(wheel_speed[3])));
	if(mm > CAN_3508_MAXrpm) {
		for(char i = 0; i < 4; ++i) wheel_speed[i] = wheel_speed[i] * CAN_3508_MAXrpm / mm;
	}
	
	for(char i = 0; i < 4; ++i) {
		motor_input[i] = (short)PID_speed_output(&wheel[i], wheel_speed[i], motor_data[i].speed_rpm);
	}
	CAN_cmd_chassis(motor_input);
}